#ifndef _world_Entity_hpp_
#define _world_Entity_hpp_

/** \file Entity.hpp
 *  \author Adrian Schweizer
 *  \created  $Do 30 Aug 04:00:24 pm CEST 2007 schwadri@SchwadriComp.local$
 *  \modified $So 28 Okt 04:40:49 pm CET 2007 schwadri@SchwadriLaptop$
 */

#include "Common.hpp"

#include "Geometry.hpp"

namespace world {

    /** \brief A physical object in 3d euclidean space
     *
     *  \note atm. we only simulate 2d behaviour for simplicity's sake. But we
     *  provide a 3d interface for future expansion.
     */
    class Entity
    {
    public:

        /** \brief Default ctor
         *
         *  Does not initialize anthing.
         */
        Entity();

        /** \brief Initialize minimally for proper function
         *
         *  \param[in] mass
         *  \param[in] inertia
         *  \param[in] x0
         *  \param[in] p0
         *  \param[in] xd0
         *  \param[in] o0
         */
        Entity(real mass, const matrix3r& inertia, const vector3r& x0, const quatr& p0, const vector3r& xd0, const vector3r& o0);

        /** \brief Construct Entity from inert parameters only
         *
         *  All positions, velocities, accelerations and forces are set to 0. The orientation quaternion is set to identity.
         */
        Entity(real mass, const matrix3r& inertia);

        /** \name Accessors*///@{
        boost::shared_ptr<Geometry> Geom() const              { return m_geom; }
        real            Mass()      const   { return m_mass; }
        const matrix3r& Inertia()   const   { return m_inertia; }
        const matrix3r& InertiaInv()const   { return m_inertia_inv; }

        const vector3r& X()     const       { return m_x; }
        const quatr&    P()     const       { return m_p; }
        const vector3r& XDot()  const       { return m_xdot; }
        const vector3r& Omega() const       { return m_omega; }
        const vector3r& Force() const       { return m_h; }
        const vector3r& Torque()const       { return m_torque; }

        const matrix4r& Transform() const   { return m_transform; }

        bool            Static() const      { return m_static; }
        //@}

        /** \name Mutators*///@{
        void Geom(boost::shared_ptr<Geometry> g)          { m_geom = g; }
        void Static(bool s)             { m_static = s; }

        void Mass(real m)               { m_mass = m; }
        void Inertia(const matrix3r& i);
        void P(const quatr& q)          { m_p = q; }        ///<set orientation quaternion
        void Omega(const vector3r& o)   { m_omega = o; }    ///<set angular velocity vector
        void X(const vector3r& x)       { m_x = x; }        ///<set position vector
        void XDot(const vector3r& xd)   { m_xdot = xd; }    ///<set linear velocty

        /** \brief Let a force act on the entity at the specified point of attack \p poa*/
        void Act(const vector3r& f, const vector3r& poa);

        /** \brief Let a (force,torque) pair act on the entity at the specified point of attack \p poa*/
        void Act(const vector6r& d, const vector3r& poa);

        /** \brief Let a force act on the entity's center of mass*/
        void Act(const vector3r& f);

        /** \brief Let a (force,torque) pair act on the entity's center of mass*/
        void Act(const vector6r& d);

        /** \brief Do euler time step*/
        void Update(real dt);

        //@}

    private:
        void UpdateTransform();
        friend class boost::serialization::access;

        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & m_type;
                ar & m_static;
                ar & m_mass;
                ar & m_inertia;
                ar & m_x;
                ar & m_p;
                ar & m_xdot;
                ar & m_omega;
            }

        int                             m_type;         ///<Entity Type Class
        bool                            m_static;       ///<True for immovable objects like walls obstacles
        boost::shared_ptr<Geometry>     m_geom;         ///<collision geometry of the entity

        /** \name Entity State*///@{
        real                            m_mass;         ///<mass \f$ m \f$
        matrix3r                        m_inertia;      ///<inertia matrix  \f$ \mathbf{M}_R \f$
        matrix3r                        m_inertia_inv;  ///<inverse inertia matrix \f$ \mathbf{M}_R^{-1} \f$
        matrix3r                        m_inertia_inv_i;///<inverse inertia matrix in inertial frame \f$ \ _{I}\mathbf{M}_R{-1} \f$

        vector3r                        m_x;            ///<position vector         \f$ \mathbf{x} \f$
        quatr                           m_p;            ///<orientation quaternion  \f$ \mathbf{p} \f$
        matrix4r                        m_transform;    ///<transformation matrix (orientation+translation) \f$ \mathbf{A_{IB}} \f$

        vector3r                        m_xdot;         ///<linear velocity vector  \f$ \mathbf{\dot{x}} \f$
        vector3r                        m_omega;        ///<angular velocity vector \f$ \mathbf{\dot{omega}} \f$
        //@}

        /** \name External Influence Accumulators*///@{
        vector3r                        m_h;            ///<external force accumulator vector \f$ \mathbf{h}_F \f$
        vector3r                        m_torque;       ///<external torque accumulator vector \f$ \mathbf{h}_R \f$
        //@}
    };

} // namespace world
#endif // _world_Entity_hpp_
